﻿using System;
using System.Collections.Generic;
using MPI;

namespace Dwarf.Managed
{
    enum SolutionType { 
        // The solution is a single loop and is exact under the given constraints.
        Exact,
        // The lower bound for the solution was greater than the upper bound,
        // so the solution was pruned.
        Pruned,
        // The lower bound was low enough, and the solution was patched 
        // using the Karp-Steele patching algorithm.
        Patched
    }
    enum MPIMessageType {
        Problem,
        Solution,
        Branches,
    }

    class BranchingInformation {
        public long LowerBound;
        public int[] ToCities;
        public int[] FromCities;
        public int[] ConstraintMap;
        public int[] Exclusions;
        public int[] u;
        public int[] v;
        private int[] childFromCities;
        private int[] childToCities;
        public int CycleCount;

        public int Count { get { return childFromCities == null ? 0 : childFromCities.Length; } }
        public int GetExcludedRow(int childIndex) {
            return childFromCities[childIndex];
        }
        public void SetChildEdges(int[] childFromCities, int[] childToCities) {
            this.childFromCities = childFromCities;
            this.childToCities = childToCities;
        }
        public static readonly BranchingInformation Empty = new BranchingInformation();

        public static int Comparer(BranchingInformation x, BranchingInformation y) {
            if (x.LowerBound < y.LowerBound)
                return 1;
            if (x.LowerBound > y.LowerBound)
                return -1;
            if (x.Count < y.Count)
                return 1;
            if (x.Count > y.Count)
                return -1;
            return 0;
        }

        public void Send(Intracommunicator incomm, int destination) {
            int n = ToCities.Length;
            int m = childFromCities.Length;
            int length = 6 + 5 * n + Exclusions.Length + 2 * m;
            // Ensure buffer is long enough for most cases
            int[] buffer = new int[(int)(6.5 * n)];
            buffer[0] = length;
            buffer[1] = CycleCount;
            buffer[2] = (int)(LowerBound >> 32);
            buffer[3] = (int)(LowerBound & 0xffffffff);
            buffer[4] = Exclusions.Length;
            buffer[5] = m;
            Array.Copy(ToCities, 0, buffer, 6, n);
            Array.Copy(FromCities, 0, buffer, 6 + n, n);
            Array.Copy(u, 0, buffer, 6 + 2 * n, n);
            Array.Copy(v, 0, buffer, 6 + 3 * n, n);
            Array.Copy(ConstraintMap, 0, buffer, 6 + 4 * n, n);
            int offset = 6 + 5 * n;
            int remaining = buffer.Length - offset;
            if (remaining >= Exclusions.Length) {
                Array.Copy(Exclusions, 0, buffer, offset, Exclusions.Length);
                remaining -= Exclusions.Length;
                if (remaining >= m) {
                    Array.Copy(childFromCities, 0, buffer, offset + Exclusions.Length, m);
                    remaining -= m;
                    if (remaining >= m) {
                        Array.Copy(childToCities, 0, buffer, offset + Exclusions.Length + m, m);
                    } else {
                        Array.Copy(childToCities, 0, buffer, offset + Exclusions.Length + m, remaining);
                        incomm.Send(buffer, destination, (int)MPIMessageType.Branches);
                        buffer = new int[m - remaining];
                        Array.Copy(childToCities, remaining, buffer, 0, m - remaining);
                    }
                } else {
                    Array.Copy(childFromCities, 0, buffer, offset + Exclusions.Length, remaining);
                    incomm.Send(buffer, destination, (int)MPIMessageType.Branches);
                    buffer = new int[2 * m - remaining];
                    Array.Copy(childFromCities, remaining, buffer, 0, m - remaining);
                    Array.Copy(childToCities, 0, buffer, m - remaining, m);
                }
            } else {
                Array.Copy(Exclusions, 0, buffer, offset, remaining);
                incomm.Send(buffer, destination, (int)MPIMessageType.Branches);
                buffer = new int[Exclusions.Length + 2 * m - remaining];
                Array.Copy(Exclusions, remaining, buffer, 0, Exclusions.Length - remaining);
                Array.Copy(childFromCities, 0, buffer, Exclusions.Length - remaining, m);
                Array.Copy(childToCities, 0, buffer, Exclusions.Length - remaining + m, m);
            }
            incomm.Send(buffer, destination, (int)MPIMessageType.Branches);
        }

        public static BranchingInformation Receive(int cityCount, Intracommunicator incomm, int source) {
            int n = cityCount;
            int[] buffer = new int[(int)(6.5 * n)];
            incomm.Receive(source, (int)MPIMessageType.Branches, ref buffer);
            int length = buffer[0];

            BranchingInformation info = new BranchingInformation();
            info.CycleCount = buffer[1];
            info.LowerBound = ((long)buffer[2] << 32) + (uint)buffer[3];
            int exclusionsLength = buffer[4];
            int childLength = buffer[5];
            info.ToCities = new int[n];
            Array.Copy(buffer, 6, info.ToCities, 0, n);
            info.FromCities = new int[n];
            Array.Copy(buffer, 6 + n, info.FromCities, 0, n);
            info.u = new int[n];
            Array.Copy(buffer, 6 + 2 * n, info.u, 0, n);
            info.v = new int[n];
            Array.Copy(buffer, 6 + 3 * n, info.v, 0, n);
            info.ConstraintMap = new int[n];
            Array.Copy(buffer, 6 + 4 * n, info.ConstraintMap, 0, n);
            info.Exclusions = new int[exclusionsLength];
            info.childFromCities = new int[childLength];
            info.childToCities = new int[childLength];
            int remaining = buffer.Length - (6 + 5 * n);
            if (remaining >= exclusionsLength) {
                Array.Copy(buffer, 6 + 5 * n, info.Exclusions, 0, exclusionsLength);
                remaining -= exclusionsLength;
                if (remaining >= childLength) {
                    Array.Copy(buffer, 6 + 5 * n + exclusionsLength, info.childFromCities, 0, childLength);
                    remaining -= childLength;
                    if (remaining >= childLength) {
                        Array.Copy(buffer, 6 + 5 * n + exclusionsLength + childLength, info.childToCities, 0, childLength);
                    } else {
                        Array.Copy(buffer, 6 + 5 * n + exclusionsLength + childLength, info.childToCities, 0, remaining);
                        incomm.Receive(source, (int)MPIMessageType.Branches, ref buffer);
                        Array.Copy(buffer, 0, info.childToCities, remaining, childLength - remaining);
                    }
                } else {
                    Array.Copy(buffer, 6 + 5 * n + exclusionsLength, info.childFromCities, 0, remaining);
                    incomm.Receive(source, (int)MPIMessageType.Branches, ref buffer);
                    Array.Copy(buffer, 0, info.childFromCities, remaining, childLength - remaining);
                    Array.Copy(buffer, childLength - remaining, info.childToCities, 0, childLength);
                }
            } else {
                Array.Copy(buffer, 6 + 5 * n, info.Exclusions, 0, remaining);
                incomm.Receive(source, (int)MPIMessageType.Branches, ref buffer);
                Array.Copy(buffer, 0, info.Exclusions, remaining, exclusionsLength - remaining);
                Array.Copy(buffer, exclusionsLength - remaining, info.childFromCities, 0, childLength);
                Array.Copy(buffer, exclusionsLength - remaining + childLength, info.childToCities, 0, childLength);
            }
            //Helper.WriteArray("rX", info.Exclusions);
            //Helper.WriteArray("rF", info.childFromCities);
            //Helper.WriteArray("rT", info.childToCities);
            return info;
        }


        private void Include(int[] constraintMap, int from, int to) {
            constraintMap[from] = 1 + to;
        }
        private void Exclude(int[] constraintMap, List<int> exclusions, int from, int to) {
            int index = constraintMap[from];
            if (index == 0) {
                // No constraints yet. Add this one at the end.
                constraintMap[from] = -1 - exclusions.Count;
                exclusions.Add(to);
                exclusions.Add(-1);
            } else if (constraintMap[from] < 0) {
                // We have a constraint. Just insert this one at the front.
                for (int i = -1 - index; exclusions[i] != -1; i++) {
                    if (exclusions[i] == to) {
                        return;
                    }
                }
                exclusions.Insert(-1 - index, to);
                for (int i = 0; i < constraintMap.Length; i++) {
                    if (i == from)
                        continue;
                    if (constraintMap[i] < index)
                        constraintMap[i]--;
                }
            }
        }

        public void GetChildConstraints(int childIndex, out int[] constraintMap, out int[] exclusions) {
            constraintMap = (int[])this.ConstraintMap.Clone();

            // Add inclusions
            for (int i = 0; i < childIndex; i++)
                Include(constraintMap, childFromCities[i], childToCities[i]);
            // Add exclusions

            List<int> exclusionList = new List<int>(this.Exclusions);
            Exclude(constraintMap, exclusionList, childFromCities[childIndex], childToCities[childIndex]);
            if (childIndex > 0) {
                Exclude(constraintMap, exclusionList, ToCities[childIndex - 1], FromCities[0]);
            }
            exclusions = exclusionList.ToArray();
        }

        public void SendProblem(int childIndex, long upperBound, Intracommunicator incomm, int destination) { 
            int n = ToCities.Length;
            int[] constraintMap, exclusions;
            GetChildConstraints(childIndex, out constraintMap, out exclusions);
            int length = 5 * n + exclusions.Length + 4;
            // Ensure buffer is long enough for most cases
            int[] buffer = new int[6 * n];
            buffer[0] = length;
            buffer[1] = GetExcludedRow(childIndex);
            buffer[2] = (int)(upperBound >> 32);
            buffer[3] = (int)(upperBound & 0xffffffff);
            Array.Copy(ToCities, 0, buffer, 4, n);
            Array.Copy(FromCities, 0, buffer, 4 + n, n);
            Array.Copy(u, 0, buffer, 4 + 2*n, n);
            Array.Copy(v, 0, buffer, 4 + 3*n, n);
            Array.Copy(constraintMap, 0, buffer, 4 + 4 * n, n);
            int remaining = buffer.Length - (4 + 5 * n);
            if (exclusions.Length <= remaining) {
                Array.Copy(exclusions, 0, buffer, 4 + 5 * n, exclusions.Length);
                incomm.Send(buffer, destination, (int)MPIMessageType.Problem);
            } else {
                Array.Copy(exclusions, 0, buffer, 4 + 5 * n, remaining);
                incomm.Send(buffer, destination, (int)MPIMessageType.Problem);
                buffer = new int[length];
                Array.Copy(exclusions, remaining, buffer, 0, exclusions.Length - remaining);
                incomm.Send(buffer, destination, (int)MPIMessageType.Problem);
            }
        }
    }

    class AssignmentProblem {
        private const int infinity = int.MaxValue;

        // Problem definition
        private int cityCount;                          // count of points
        private int[,] distances;                       // square matrix of distances between all points

        private int[] constraintMap;
        // constraintMap[i] >  0 -> (i, constraintMap[i]-1) is Included
        // constraintMap[i] == 0 -> no constraints on (i, ?)
        // constraintMap[i] <  0 -> -1 - constraintMap[i] = index of first element in exclusions list.
        private int[] exclusions;
        // See constraintMap. List is terminated by -1.

        private int[] patchedToCities;
        private int[] patchedFromCities;
        private long patchedValue;
        private int[] toCities;                         // final array of cities
        private int[] fromCities;                       // originating city
        private long upperBound = infinity;                     // final sum of distances between cities
        private long lowerBound = infinity;                     // final sum of distances between cities
        private int excludedRow;
        private bool isRoot;

        // Dual variable associated with each row:
        int[] u;
        // Dual variable associated with each column:
        int[] v;

        public AssignmentProblem(int cityCount, int[,] distances) {
            this.cityCount = cityCount;
            this.distances = distances;
            this.constraintMap = new int[cityCount];
            this.exclusions = new int[0];
            this.upperBound = infinity;
            this.isRoot = true;
        }

        public AssignmentProblem(int cityCount, int[,] distances, BranchingInformation info, int childIndex, long upperBound) {
            this.cityCount = cityCount;
            this.distances = distances;

            this.toCities = (int[])info.ToCities.Clone();
            this.fromCities = (int[])info.FromCities.Clone();
            info.GetChildConstraints(childIndex, out this.constraintMap, out this.exclusions);
            this.u = (int[])info.u.Clone();
            this.v = (int[])info.v.Clone();

            this.excludedRow = info.GetExcludedRow(childIndex);
            this.upperBound = upperBound;
            this.isRoot = false;
        }

        public static AssignmentProblem Receive(int cityCount, int[,] distances, Intracommunicator incomm, int source) {
            int n = cityCount;
            int[] buffer = new int[6 * n];
            incomm.Receive(source, (int)MPIMessageType.Problem, ref buffer);
            int length = buffer[0];

            if (length == 0)
                return null;

            AssignmentProblem problem = new AssignmentProblem(cityCount, distances);
            problem.isRoot = false;
            problem.excludedRow = buffer[1];
            problem.upperBound = ((long)buffer[2] << 32) + (uint)buffer[3];
            problem.toCities = new int[n];
            Array.Copy(buffer, 4, problem.toCities, 0, n);
            problem.fromCities = new int[n];
            Array.Copy(buffer, 4 + n, problem.fromCities, 0, n);
            problem.u = new int[n];
            Array.Copy(buffer, 4 + 2 * n, problem.u, 0, n);
            problem.v = new int[n];
            Array.Copy(buffer, 4 + 3 * n, problem.v, 0, n);
            problem.constraintMap = new int[n];
            Array.Copy(buffer, 4 + 4 * n, problem.constraintMap, 0, n);
            int exclusionsLength = length - (4 + 5 * n);
            problem.exclusions = new int[exclusionsLength];
            int remaining = buffer.Length - (4 + 5 * n);
            if (remaining >= exclusionsLength) {
                Array.Copy(buffer, 4 + 5 * n, problem.exclusions, 0, exclusionsLength);
            } else {
                Array.Copy(buffer, 4 + 5 * n, problem.exclusions, 0, remaining);
                buffer = new int[length];
                incomm.Receive(source, (int)MPIMessageType.Problem, ref buffer);
                Array.Copy(buffer, 0, problem.exclusions, remaining, exclusionsLength - remaining);
            }
            return problem;
        }

        public long LowerBound { get { return lowerBound; } }
        public long Value { get { return patchedValue; } }
        public int[] GetCities() {
            return patchedToCities ?? toCities;
        }

        public void SendPrunedSolution(Intracommunicator incomm, int destination) {
            int[] buffer = { (int)SolutionType.Pruned };
            incomm.Send(buffer, destination, (int)MPIMessageType.Solution);
        }
        public void SendExactSolution(Intracommunicator incomm, int destination) {
            int n = cityCount;
            int length = 4 + 2 * n;
            int[] buffer = new int[length];
            buffer[0] = (int)SolutionType.Exact;
            buffer[2] = (int)(lowerBound >> 32);
            buffer[3] = (int)(lowerBound & 0xffffffff);
            Array.Copy(toCities, 0, buffer, 4, n);
            Array.Copy(fromCities, 0, buffer, 4 + n, n);
            incomm.Send(buffer, destination, (int)MPIMessageType.Solution);
        }
        public void SendPatchedSolution(Intracommunicator incomm, int destination) {
            int n = cityCount;
            int length = 4 + 2 * n;
            int[] buffer = new int[length];
            buffer[0] = (int)SolutionType.Patched;
            buffer[2] = (int)(patchedValue >> 32);
            buffer[3] = (int)(patchedValue & 0xffffffff);
            Array.Copy(patchedToCities, 0, buffer, 4, n);
            Array.Copy(patchedFromCities, 0, buffer, 4 + n, n);
            incomm.Send(buffer, destination, (int)MPIMessageType.Solution);
        }

        /// <summary>
        /// Solves the assignment problem and returns the total distance.
        /// </summary>
        public long Solve() {
            if (lowerBound < infinity)
                return lowerBound;

            bool success;
            if (isRoot)
                success = SolveFromScratch();
            else 
                success = SolveIncremental();

            if (!success)
                return infinity;

            // Compute total distance.
            this.lowerBound = 0;
            for (int i = 0; i < cityCount; i++)
                lowerBound += u[i] + v[i];

            // Extract cycles
            ComputeCycles();
            if (cycles.Count == 1)
                patchedValue = lowerBound;
            return lowerBound;
        }

        #region Constraints

        private bool IsAllowed(int row, int column) {
            int value = constraintMap[row];
            return (row != column) && ((value == 0) ? (row != column) : IsAllowedConstraint(value, column));
        }
        private bool IsAllowedConstraint(int value, int column) {
            if (value > 0)
                // Include
                return column == (value - 1);
            else {
                // Exclude
                value = -1 - value;
                while (exclusions[value] >= 0) {
                    if (exclusions[value] == column)
                        return false;
                    value++;
                }
                return true;
            }
        }
        #endregion

        private bool SolveFromScratch() {
            this.toCities = new int[cityCount];
            this.fromCities = new int[cityCount];
            this.u = new int[cityCount];
            this.v = new int[cityCount];

            // Label all cities as unassigned
            int assignedCities = 0;
            for (int i = 0; i < cityCount; i++) {
                toCities[i] = -1;
                fromCities[i] = -1;
            }

            // Find the smallest distance in each column.
            for (int column = 0; column < cityCount; column++) {
                // Find the i distance in the column.
                int minDistance = infinity;
                int minRow = -1;
                for (int row = 0; row < cityCount; row++) {
                    if (!IsAllowed(row, column))
                        continue;
                    int distance = distances[row, column];
                    if (distance < minDistance || (distance == minDistance && toCities[row] < 0)) {
                        minRow = row;
                        minDistance = distance;
                    }
                }
                if (minRow < 0)
                    return false;

                // This distance is the value of the dual variable of the column.
                v[column] = minDistance;

                // If the row is unassigned, we can assign it.
                if (toCities[minRow] < 0) {
                    // Assign column to row minRow
                    assignedCities++;
                    fromCities[column] = minRow;
                    toCities[minRow] = column;
                    u[minRow] = 0;
                }
            }

            // Now look through unassigned rows, 
            // and see if they can be assigned.
            for (int row = 0; row < cityCount; row++) {
                if (toCities[row] >= 0)
                    continue;

                int minReducedCost = infinity;
                int minColumn = -1;
                for (int column = 0; column < cityCount; column++) {
                    if (!IsAllowed(row, column))
                        continue;
                    int reducedCost = distances[row, column] - v[column];
                    if (reducedCost < minReducedCost
                        || (reducedCost == minReducedCost && fromCities[column] < 0 && fromCities[minColumn] >= 0)) {
                        minReducedCost = reducedCost;
                        minColumn = column;
                    }
                }

                if (minColumn < 0)
                    return false;

                u[row] = minReducedCost;

                if (fromCities[minColumn] < 0) {
                    // Assign minColumn to row i
                    assignedCities++;
                    toCities[row] = minColumn;
                    fromCities[minColumn] = row;
                }
            }

            if (assignedCities < cityCount) {
                AllocateAugmentingPathTemporaries();
                for (int row = 0; row < cityCount; row++) {
                    if (toCities[row] >= 0)
                        continue;
                    FindAugmentingPath(row);
                }
                FreeAugmentingPathTemporaries();
            }
            return true;
        }


        private bool SolveIncremental() {
            int row = this.excludedRow;
            int column = toCities[row];
            fromCities[column] = -1;
            toCities[row] = -1;

            AllocateAugmentingPathTemporaries();
            FindAugmentingPath(row);
            FreeAugmentingPathTemporaries();
            return true;
        }

        int[] pathOrigins;
        int[] reducedCosts;
        int[] labeledRow;
        int[] unlabeledColumn;

        void AllocateAugmentingPathTemporaries() {
            pathOrigins = new int[cityCount];
            reducedCosts = new int[cityCount];
            labeledRow = new int[cityCount];
            unlabeledColumn = new int[cityCount];
        }
        void FreeAugmentingPathTemporaries() {
            pathOrigins = null;
            reducedCosts = null;
            labeledRow = null;
            unlabeledColumn = null;
        }

        void FindAugmentingPath(int row) {

            labeledRow[0] = row;
            for (int column = 0; column < cityCount; column++) {
                if (!IsAllowed(row, column))
                    reducedCosts[column] = infinity;
                else
                    reducedCosts[column] = distances[row, column] - u[row] - v[column];
                pathOrigins[column] = row;
                unlabeledColumn[column] = column;
            }
            int unlabeledColumnCount = cityCount;
            int labeledRowCount = 1;

            for (; ; ) {
                // Find a zero in an unlabeled column
                bool foundZero = false;
                for (int l = 0; l < unlabeledColumnCount; l++) {
                    int column = unlabeledColumn[l];
                    if (reducedCosts[column] == 0) {
                        foundZero = true;

                        // If the column is unassigned, we're done.
                        if (fromCities[column] < 0) {
                            do {
                                int origin = pathOrigins[column];
                                fromCities[column] = origin;
                                int hold = toCities[origin];
                                toCities[origin] = column;
                                column = hold;
                            } while (column >= 0);
                            return;
                        }

                        // label the row corresponding to the column.
                        labeledRow[labeledRowCount++] = fromCities[column];
                        // remove label from column column
                        unlabeledColumn[l] = unlabeledColumn[--unlabeledColumnCount];
                        break;
                    }
                }
                if (foundZero) {
                    // Scan labeled rows
                    int r = labeledRow[labeledRowCount - 1];
                    for (int l = 0; l < unlabeledColumnCount; l++) {
                        int column = unlabeledColumn[l];
                        if (!IsAllowed(r, column))
                            continue;
                        int reducedCost = distances[r, column] - u[r] - v[column];
                        if (reducedCost < reducedCosts[column]) {
                            reducedCosts[column] = reducedCost;
                            pathOrigins[column] = r;
                        }
                    }
                } else {
                    // Update the dual variables
                    // Find minimum value.
                    int minReducedCost = infinity;
                    for (int l = 0; l < unlabeledColumnCount; l++) {
                        int column = unlabeledColumn[l];
                        if (reducedCosts[column] < minReducedCost)
                            minReducedCost = reducedCosts[column];
                    }
                    // Add minimum to dual variables of labeled rows
                    for (int l = 0; l < labeledRowCount; l++) {
                        int r = labeledRow[l];
                        u[r] += minReducedCost;
                    }
                    // Subtract minimum from dual variables of labeled columns
                    for (int column = 0; column < cityCount; column++) {
                        if (reducedCosts[column] == 0)
                            v[column] -= minReducedCost;
                        else
                            reducedCosts[column] -= minReducedCost;
                    }
                }

            }
        }

        #region Patching/merging of cycles
        struct Cycle {
            public int StartNode;   // First city in the cycle
            public int Length;      // Total # cities
            public int FreeLength;  // # cities not in the Include list
        }
        List<Cycle> cycles;

        /// <summary>
        /// Gets the starting point of the cycle of i length.
        /// </summary>
        /// <returns></returns>
        private void ComputeCycles() {
            bool[] visited = new bool[cityCount];
            this.cycles = new List<Cycle>();
            int cycleCount = 0;
            for (int i = 0; i < visited.Length; i++) {
                if (visited[i])
                    continue;
                cycleCount++;
                visited[i] = true;
                int next = toCities[i];
                int length = 1;
                int freeLength = constraintMap[i] > 0 ? 0 : 1;
                while (next != i) {
                    visited[next] = true;
                    length++;
                    if (constraintMap[next] <= 0)
                        freeLength++;
                    next = toCities[next];
                }
                Cycle cycle = new Cycle();
                cycle.Length = length;
                cycle.FreeLength = freeLength;
                cycle.StartNode = i;
                this.cycles.Add(cycle);
            }

            if (this.cycles.Count > 1) {
                this.cycles.Sort((x, y) => x.Length > y.Length ? 1 : x.Length < y.Length ? -1 : 0);
            }
        }

        public int CycleCount { get { return cycles.Count; } }

        private void Patch(int from, int to, int from2, int to2) {
            patchedToCities[from] = to2;
            patchedFromCities[to2] = from;
            patchedToCities[from2] = to;
            patchedFromCities[to] = from2;
        }

        /// <summary>
        /// Patches the cycles with the specified start cities using the
        /// Karp-Steele patch algorithm.
        /// </summary>
        /// <param name="startCity"></param>
        /// <param name="startCity2"></param>
        /// <returns>The cost of the patch.</returns>
        private long Patch(int startCity, int startCity2) {
            int bestLast = -1;
            int bestLast2 = -1;
            int last = startCity;
            int next = toCities[last];
            long minPenalty = long.MaxValue;
            while (next != startCity) {

                long basePenalty = -distances[last, next];
                int last2 = startCity2;
                int next2 = toCities[last2];
                while (next2 != startCity2) {
                    if (IsAllowed(last, next2) && IsAllowed(last2, next)) {
                        if (distances[last, next2] - u[last] - v[next2] == 0
                            && distances[last2, next] - u[last2] - v[next] == 0) {
                            // Found them!

                            Patch(last, next, last2, next2);
                            return 0;
                        }
                        long penalty = basePenalty - distances[last2, next2]
                            + distances[last, next2] + distances[last2, next];
                        if (penalty < minPenalty) {
                            minPenalty = penalty;
                            bestLast = last;
                            bestLast2 = last2;
                        }
                    }
                    last2 = next2;
                    next2 = toCities[last2];
                }
                last = next;
                next = toCities[last];
            }

            if (bestLast < 0 || bestLast2 < 0)
                return infinity;
            Patch(bestLast, patchedToCities[bestLast], bestLast2, patchedToCities[bestLast2]);
            return minPenalty;
        }

        public long Patch(Solver solver) {
            patchedToCities = (int[])toCities.Clone();
            patchedFromCities = (int[])fromCities.Clone();

            long penalty = 0;
            long maxPenalty = this.upperBound - this.lowerBound;
            for (int cycle = cycles.Count - 1; cycle > 0; --cycle) {
                int startCity = cycles[cycle].StartNode;
                int startCity2 = cycles[cycle - 1].StartNode;
                long nextPenalty = Patch(startCity, startCity2);
                if (nextPenalty == infinity) {
                    penalty = infinity;
                    break;
                }
                penalty += nextPenalty;
                if (penalty >= maxPenalty) {
                    break;
                }
                if (solver != null)
                    solver.ProcessMessages(false);
            }
            this.patchedValue = penalty == infinity ? infinity : this.lowerBound + penalty;
            return penalty;
        }
        #endregion

        #region Branching
        public BranchingInformation GetBranchingInformation() {

            BranchingInformation info = new BranchingInformation();

            info.LowerBound = this.LowerBound;
            info.FromCities = this.fromCities;
            info.ToCities = this.toCities;
            info.ConstraintMap = this.constraintMap;
            info.Exclusions = this.exclusions;
            info.u = this.u;
            info.v = this.v;
            info.CycleCount = this.CycleCount;

            // Find cycle with the least included branches.
            int minLength = int.MaxValue;
            int shortest = -1;
            for (int i = 0; i < cycles.Count; i++) {
                if (cycles[i].FreeLength < minLength) {
                    shortest = i;
                    minLength = cycles[i].FreeLength;
                }
            }

            int[] childFromCities = new int[minLength];
            int[] childToCities = new int[minLength];

            int last = cycles[shortest].StartNode;
            int next;
            int index = 0;
            do {
                next = toCities[last];
                if (constraintMap[last] <= 0) {
                    childFromCities[index] = last;
                    childToCities[index++] = next;
                }
                last = next;
            } while (last != cycles[shortest].StartNode);

            info.SetChildEdges(childFromCities, childToCities);
            return info;
        }
        #endregion
    }

    /// <summary>Solve the Traveller problem using 'Branch And Bound' method with MPI parallelization</summary>
    ///  
    class Solver
    {                                                   // MPI variables:
        MPI.Environment mpienv;                         // Enviroment
        MPI.Intracommunicator incomm;                   // Intracommunicator
        int anyTag = Communicator.anyTag;               // Tag for sending/receiving
        int rank;                                       // rank of the process
        int commsize;                                   // count of processes
        int root = 0;                                   // index of root process. By consideration, equal to 0

        private Configurator dwarfConfigurator;
        private int cityCount;                          // count of points
        private int[,] distances;                       // square matrix of distances between all points
        private const int infinity = int.MaxValue;

        /// <summary>Construct the new solver</summary>
        /// <param name="dwarfConfigurator">Processed command line arguments</param>
        /// <param name="args">Command line arguments for MPI environment</param>
        /// 
        public Solver(ref Configurator dwarfConfigurator, ref string[] args)
        {
            mpienv = new MPI.Environment(ref args);     // MPI Initialization
            rank = MPI.Communicator.world.Rank;         // Rank of the sproces
            commsize = MPI.Communicator.world.Size;     // Count of processes
            incomm = MPI.Communicator.world;

            if (rank == root)
            {
                this.dwarfConfigurator = dwarfConfigurator;
                dwarfConfigurator.WriteSettings();                      // write settings to console and profile file
                dwarfConfigurator.GetData(ref cityCount, ref distances);// getting the array of edge values

                solvedList = new List<BranchingInformation>();
            }
        }

        #region Shared between root and other nodes
        private SolutionType Solve(AssignmentProblem problem) {
            long lowerBound = problem.Solve();
            if (lowerBound >= upperBound) {
                if (rank != root)
                    problem.SendPrunedSolution(incomm, root);
                return SolutionType.Pruned;
            } else if (lowerBound < upperBound && problem.CycleCount == 1) {
                if (rank != root)
                    problem.SendExactSolution(incomm, root);
                return SolutionType.Exact;
            } else {
                if (rank == root)
                    ProcessMessages(false);
                problem.Patch(rank == root ? this : null);
                if (rank != root) {
                    problem.SendPatchedSolution(incomm, root);
                    problem.GetBranchingInformation().Send(incomm, root);
                }
                return SolutionType.Patched;
            }
        }
        #endregion

        #region Root specific
        private int[] bestCities;
        private long upperBound;
        private List<BranchingInformation> solvedList;

        private bool[] idle;
        private int activeNodes;

        BranchingInformation currentBranchingInfo;
        int currentBranchingIndex;

        /// <summary>
        /// Gets the next problem. This method determines the search order.
        /// </summary>
        AssignmentProblem GetNextProblem() {
            AssignmentProblem problem = new AssignmentProblem(cityCount, distances, currentBranchingInfo, currentBranchingIndex, upperBound);
            currentBranchingIndex++;
            return problem;
        }
        void SendNextProblem(int destination) {
            currentBranchingInfo.SendProblem(currentBranchingIndex, upperBound, incomm, destination);
            currentBranchingIndex++;
        }

        bool MoreProblems(bool wait) {
            if (currentBranchingIndex < currentBranchingInfo.Count)
                return true;
            CreateSubProblems();
            if (currentBranchingIndex < currentBranchingInfo.Count)
                return true;
            while (wait && activeNodes > 0) {
                ProcessMessages(true);
                CreateSubProblems();
                if (currentBranchingIndex < currentBranchingInfo.Count)
                    return true;
            }
            return false;
        }
        void CreateSubProblems() {
            // Prune solved problems
            int index = 0;
            while (index < solvedList.Count && solvedList[index].LowerBound >= upperBound)
                index++;
            solvedList.RemoveRange(0, index);

            if (solvedList.Count == 0) {
                currentBranchingIndex = infinity;
                currentBranchingInfo = BranchingInformation.Empty;
                return;
            }

            index = solvedList.Count - 1;
            currentBranchingInfo = solvedList[index];
            currentBranchingIndex = 0;
            solvedList.RemoveAt(index);
        }
        void AddBranchingInformation(BranchingInformation info) {
            int index = 0;
            while (index < solvedList.Count && BranchingInformation.Comparer(solvedList[index], info) <= 0)
                index++;
            solvedList.Insert(index, info);
        }

        public void ProcessMessages(bool wait) { 
            MPI.Status status = incomm.ImmediateProbe(MPI.Communicator.anySource, (int)MPIMessageType.Solution);
            while (status != null || (wait && activeNodes > 0)) {
                if (status == null) {
                    status = incomm.Probe(MPI.Communicator.anySource, (int)MPIMessageType.Solution);
                }

                // Receive solution
                int[] buffer = new int[4 + 2 * cityCount];
                incomm.Receive(status.Source, (int)MPIMessageType.Solution, ref buffer);
                SolutionType type = (SolutionType)buffer[0];
                switch (type) {
                    case SolutionType.Pruned:
                        break;
                    case SolutionType.Exact:
                    case SolutionType.Patched:
                        long value = ((long)buffer[2] << 32) + buffer[3];
                        if (value < upperBound) {
                            upperBound = value;
                            int[] toCities = new int[cityCount];
                            Array.Copy(buffer, 4, bestCities, 0, cityCount);
                            Console.WriteLine(upperBound);
                        }

                        if (type == SolutionType.Patched) {
                            BranchingInformation info = BranchingInformation.Receive(cityCount, incomm, status.Source);
                            AddBranchingInformation(info);
                        }
                        break;
                }

                activeNodes--;
                idle[status.Source] = true;

                if (MoreProblems(false)) {
                    SendNextProblem(status.Source);
                    idle[status.Source] = false;
                    activeNodes++;
                }
                status = incomm.ImmediateProbe(MPI.Communicator.anySource, (int)MPIMessageType.Solution);
            }
        }

        AssignmentProblem currentProblem;
        private void SolveRoot() {
            activeNodes = 0;
            idle = new bool[incomm.Size];
            for (int i = 0; i < idle.Length; i++)
                idle[i] = true;

            // First, solve the initial problem.
            AssignmentProblem first = new AssignmentProblem(cityCount, distances);
            long lowerBound = first.Solve();
            if (first.CycleCount == 1) {
                // Our first try is the best solution!
                bestCities = first.GetCities();
                upperBound = lowerBound;
            } else {
                // Find an initial upper bound
                first.Patch(null);
                bestCities = first.GetCities();
                upperBound = first.Value;

                // And create sub-problems
                currentBranchingInfo = first.GetBranchingInformation();

                // Dispatch loop
                while (MoreProblems(true)) {
                    // Process any waiting solution messages
                    ProcessMessages(false);

                    // Assign jobs to idle nodes
                    for (int i = 1; i < idle.Length; i++) {
                        if (!idle[i])
                            continue;
                        if (!MoreProblems(false))
                            break;
                        SendNextProblem(i);
                        idle[i] = false;
                        activeNodes++;
                    }

                    // If we have more problems to do,
                    // solve the next one.
                    if (MoreProblems(false)) {
                        currentProblem = GetNextProblem();
                        SolutionType type = Solve(currentProblem);
                        switch (type) {
                            case SolutionType.Pruned:
                                // Nothing to do
                                break;
                            case SolutionType.Exact:
                            case SolutionType.Patched:
                                if (currentProblem.Value < upperBound) {
                                    bestCities = currentProblem.GetCities();
                                    upperBound = currentProblem.Value;
                                    Console.WriteLine(upperBound);
                                }
                                if (type == SolutionType.Patched) {
                                    BranchingInformation info = currentProblem.GetBranchingInformation();
                                    AddBranchingInformation(info);
                                }
                                break;
                        }
                    }
                }
            }

            // Send empty problems to all nodes to signal that we're done.
            int[] buffer = { 0 };
            for (int i = 0; i < incomm.Size; i++)
                if (i != root)
                    incomm.Send(buffer, i, (int)MPIMessageType.Problem);
        }
        #endregion

        #region Node specific
        private void SolveNode() {
            for (; ; ) {
                AssignmentProblem problem = AssignmentProblem.Receive(cityCount, distances, incomm, root);
                if (problem == null)
                    break;
                Solve(problem);
            }
        }
        #endregion

        public long UpperBound { get { return this.upperBound; } }

        /// <summary>MPI-parallelized method of solving. The algoritm requires only 2 processors</summary>
        ///
        public void Solve()
        {
            incomm.Broadcast(ref this.distances, root);
            this.cityCount = this.distances.GetLength(0);

            upperBound = infinity;
            if (rank == root)
                SolveRoot();
            else
                SolveNode();
        }

        /// <summary>Problem results output</summary>
        /// <param name="time">Spent time</param>
        ///
        public void Finish(double time)
        {
            if (rank == root)
            {
                dwarfConfigurator.Close(time, bestCities, cityCount);
                Console.WriteLine("Total Distance    : " + upperBound);
            }
            mpienv.Dispose();
        }
    }
}